#include "imu.hpp"

Imu::Imu(std::shared_ptr<ros::NodeHandle> node_handle_ptr,
         const std::string &serial_port)
    : node_handle_ptr_(node_handle_ptr), Serial(serial_port) {
    name_ = serial_port.substr(5);
    publisher_ = node_handle_ptr_->advertise<sensor_msgs::Imu>(name_, 1);
    switch_ =
        node_handle_ptr_->advertiseService(name_, &Imu::SwitchCallBack, this);
    //   LOG(ERROR) << name_ << " imu frame size:(" << sizeof(ImuFrame) << ")";
}

void Imu::ImuRxThread() {
    ros::Rate rate(kImuFrequence);
    std::vector<uint8_t> rx_data;
    LOG(ERROR) << name_ << " thread started!";
    while (true) {
        if (is_running_ && ReadFromIO(rx_data))
            ParseData(rx_data);
        rate.sleep();
    }
}

void Imu::ParseData(std::vector<uint8_t> &data) {
    auto get_frame_crc = [&](const std::vector<uint8_t> &data) {
        uint16_t summary = 0;
        if (data.size() != sizeof(ImuFrame))
            return summary;
        for (size_t i = 1; i < sizeof(ImuFrame) - 4; i++)
            summary += data[i];
        return summary;
    };

    while (data.size() >= sizeof(ImuFrame)) {
        size_t drop = 0;
        while (data.size() >= sizeof(ImuFrame)) {
            if (data[0] == kImuHeader &&
                data[sizeof(ImuFrame) - 2] == kImuEnd1 &&
                data[sizeof(ImuFrame) - 1] == kImuEnd2) {
                size_t length =
                    size_t(uint16_t(data[6]) << 8 | uint16_t(data[5]));
                if (length != 92)
                    LOG(ERROR) << "length: (" << length << "),sizeof frame:("
                               << sizeof(ImuFrame) << ")";
                break;
            } else {
                drop++;
                data.erase(data.begin());
            }
        };
        if (drop > 0)
            LOG(ERROR) << name_ << " drop size:(" << drop << ").";
        if (data.size() < sizeof(ImuFrame))
            return;
        std::vector<uint8_t> imu_data(data.begin(),
                                      data.begin() + sizeof(ImuFrame));
        data.erase(data.begin(), data.begin() + sizeof(ImuFrame));
        ImuFrame *imu_frame = reinterpret_cast<ImuFrame *>(imu_data.data());
        if (get_frame_crc(imu_data) != imu_frame->crc) {
            LOG(ERROR) << name_ << " frame crc error.";
            return;
        }
        sensor_msgs::Imu imu;
        imu.header.stamp = ros::Time::now();
        imu.header.frame_id = name_;
        imu.linear_acceleration.x = imu_frame->corrected_accel[0];
        imu.linear_acceleration.y = imu_frame->corrected_accel[1];
        imu.linear_acceleration.z = imu_frame->corrected_accel[2];
        imu.linear_acceleration_covariance = {0.04, 0, 0, 0,   0.04,
                                              0,    0, 0, 0.04};
        imu.angular_velocity.x = imu_frame->corrected_gyro[0];
        imu.angular_velocity.y = imu_frame->corrected_gyro[1];
        imu.angular_velocity.z = imu_frame->corrected_gyro[2];
        imu.angular_velocity_covariance = {0.02, 0, 0, 0, 0.02, 0, 0, 0, 0.02};
        imu.orientation.x = imu_frame->quaternion[0];
        imu.orientation.y = imu_frame->quaternion[1];
        imu.orientation.z = imu_frame->quaternion[2];
        imu.orientation.w = imu_frame->quaternion[3];
        imu.orientation_covariance = {1e6, 0, 0, 0, 1e6, 0, 0, 0, 0.05};
        Publish(imu);
    };
}

void Imu::Publish(const sensor_msgs::Imu &imu) {
    publisher_.publish(imu);
    static auto last = ros::Time::now();
    last = imu.header.stamp;
    times_.push_back(last);
    while (times_.size() > 100) {
        times_.erase(times_.begin());
        LOG_EVERY_N(ERROR, 100)
            << name_ << " publish frequence in last 100 message:("
            << double(times_.size() - 1) /
                   ((times_.back() - times_.front()).toSec())
            << ")hz.";
    }
}

bool Imu::SetRunning(const bool &running) {
    ros::Rate rate(kImuFrequence);
    LOG(ERROR) << "SetRunning:(" << running << "),is_start:(" << is_start_
               << ")";
    if (running) {
        Close();
        Open(kBandrateCode115200);
        is_running_ = true;
        return true;
    } else {
        Close();
        is_running_ = false;
        return true;
    }
}

bool Imu::SwitchCallBack(std_srvs::SetBool::Request &req,
                         std_srvs::SetBool::Response &res) {
    if (SetRunning(req.data)) {
        res.success = true;
        return true;
    } else {
        res.success = false;
        return false;
    }
}